/********************** (C) COPYRIGHT 2007-2009 RAISONANCE ********************
* File Name          :  Application.c
* Author             :
* Date First Issued  :
* Description        :  Circle_App CircleOS application template.
* Revision           :
*******************************************************************************/

/* Includes ------------------------------------------------------------------*/
#include "EarlTesting.h"

/* Private defines -----------------------------------------------------------*/

// The following should be the minimal CircleOS version needed by your application
#define NEEDEDVERSION "V 3.7"
#define abs(a) ((0) <= (a) ? (a) : (-a))

const char Application_Name[8+1] = {"iAROC"};      // Max 8 characters
u32 microSeconds = 0;
u32 oldPLL = 0;
CREATE_DATA cd;

GLOBALS globals;


/*******************************************************************************
* Function Name  : Application_Ini
* Description    : Initialization function of Circle_App. This function will
*                  be called only once by CircleOS.
* Input          : None
* Return         : MENU_CONTINUE_COMMAND
*******************************************************************************/
enum MENU_code Application_Ini(void)
    {
    // Ensure that the current OS version is recent enough
    if(strcmp(UTIL_GetVersion(), NEEDEDVERSION) < 0)
    {
//        return MsgVersion();
    }

    /* Set the clock to the maximum of 72 MHz */
    u8 oldPLL = UTIL_GetPll();
    UTIL_SetPll( SPEED_VERY_HIGH );
        
    // TODO: Write your application initialization function here.
    DRAW_Clear();
    DRAW_DisplayString(0, 100, "F :", 3);
    DRAW_DisplayString(0, 85, "RF:", 3);
    DRAW_DisplayString(0, 70, "RR:", 3);
    DRAW_DisplayString(0, 55, "D :", 3);
    DRAW_DisplayString(0, 40, "A :", 3);

    TIM1_Init();
    CREATE_Init();
    GLOBALS_Init();
    
    return MENU_CONTINUE_COMMAND;
    }

/*******************************************************************************
* Function Name  : Application_Handler
* Description    : Management of the Circle_App.
*
* Input          : None
* Return         : MENU_CONTINUE
*******************************************************************************/
enum MENU_code Application_Handler(void)
    {
    u32 time;
    static u8 i;

    // TODO: Write your application handling here.


    // This routine will get called repeatedly by CircleOS, until we
    // return MENU_LEAVE

#if 1
    // If the button is pressed, the application is exited
    if(BUTTON_GetState() == BUTTON_PUSHED)
        {
        BUTTON_WaitForRelease();
        Create_Start(USART1, MODE_SAFE);
        UTIL_SetPll( oldPLL );
        return MENU_Quit();
        }
#endif
    updateSensors();
    if( TOUCHSCR_IsPressed() == 1 )
        followTheWall_grid();

    updateStatus();
    return MENU_CONTINUE;   // Returning MENU_LEAVE will quit to CircleOS
    }

/*******************************************************************************
* Function Name  : MsgVersion
* Description    : Display the current CircleOS version and the version needed
*                  exit to main menu after 4 secondes
*
* Input          : None
* Return         : MENU_REFRESH
*******************************************************************************/
static enum MENU_code MsgVersion(void)
   {
   int hh,mm,ss,ss2;
   
   DRAW_DisplayString(5,60,"CircleOS",17);
   DRAW_DisplayString(80,60,UTIL_GetVersion(),6);
   DRAW_DisplayString(5,34,NEEDEDVERSION,6);
   DRAW_DisplayString(50,34," required",12);
   
   RTC_GetTime( &hh, &mm, &ss);
   ss = ss + 4;                  // 4 secondes 
   ss = ss%60;
   
   do
        {
        RTC_GetTime( &hh, &mm, &ss2 );
        }
   while ( ss2 != ss );           // do while < 4 secondes
   
   DRAW_Clear();
   return MENU_REFRESH;
   }


/*******************************************************************************
* Function Name  : USART1_Init
* Description    : Initializes USART1 at 57,600bps for communicating with the 
*                  iRobot Create.
*
* Input          : None
* Return         : None
*******************************************************************************/
void USART1_Init( void )
{
    GPIO_InitTypeDef GPIO_InitStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO | RCC_APB2Periph_USART1, ENABLE);

    GPIO_PinRemapConfig(GPIO_Remap_USART1, ENABLE);

    /* Configure USART1 Tx (PB.6) as alternate function push-pull */
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    /* Configure USART1 Rx (PB.7) as input floating */
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOB, &GPIO_InitStructure);


    USART_InitTypeDef USART_InitStructure;
    USART_InitStructure.USART_BaudRate = 57600;
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
    USART_InitStructure.USART_StopBits = USART_StopBits_1;
    USART_InitStructure.USART_Parity = USART_Parity_No;
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
    USART_Init(USART1, &USART_InitStructure);
    
//    USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
    
    /* Enable the USART1 */
    USART_Cmd(USART1, ENABLE);
}


/*******************************************************************************
* Function Name  : TIM1_Init
* Description    : Initializes TIM1 to increment every 1usec and enables the  
*                  update interrupt.
*
* Input          : None
* Return         : None
*******************************************************************************/
void TIM1_Init( void )
{
    TIM_TimeBaseInitTypeDef TIM_InitStructure;

    /* Enable TIM1 Clock */
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, DISABLE);

    /* Enable the Interrupt handler for TIM1_Update */
    UTIL_SetIrqHandler(0xA4, (u32)TIM1_Irq);
    UTIL_SetIrqHandler(0xD4, (u32)USART1_IRQHandler);
    NVIC_Configuration();

    RCC_ClocksTypeDef RCC_ClocksStatus;
    RCC_GetClocksFreq(&RCC_ClocksStatus);

    /* Setup timer to count in 1us increments */
    TIM_InitStructure.TIM_Period = 0xFFFF;
    TIM_InitStructure.TIM_Prescaler = RCC_ClocksStatus.SYSCLK_Frequency / 1000000;
    TIM_InitStructure.TIM_ClockDivision = TIM_CKD_DIV2;
    TIM_InitStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_InitStructure.TIM_RepetitionCounter = 0;

    TIM_TimeBaseInit(TIM1, &TIM_InitStructure);
    TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE );
    TIM_Cmd(TIM1, ENABLE);
    
    TIM1->CR1 |= CR1_CEN_Set;
}


/*******************************************************************************
* Function Name  : NVIC_Configuration
* Description    : Configure the nested vectored interrupt controller.
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void NVIC_Configuration(void)
{
    NVIC_InitTypeDef NVIC_InitStructure;

    /* Enable the TIM1 Up event Interrupt */
    NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_IRQChannel;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQChannel;
    NVIC_Init(&NVIC_InitStructure);
}


/*******************************************************************************
* Function Name  : CREATE_Init
* Description    : Calls USART1_Init() to setup the serial communication and 
*                  sens the create the startup commands to put it in full mode.
*
* Input          : None
* Return         : None
*******************************************************************************/
void CREATE_Init( void )
{
    USART1_Init();
    Create_Start(USART1, CMD_FULL);
}


/*******************************************************************************
* Function Name  : GLOBALS_Init
* Description    : Initializes the global variables to their defaults.
*               
*
* Input          : None
* Return         : None
*******************************************************************************/
void GLOBALS_Init( void )
{
    globals.frontPing = 0;
    globals.rfPing = 0;
    globals.rrPing = 0;
    globals.leftVel = 0;
    globals.rightVel = 0;
    globals.distance = 0;
    globals.angle = 0;
}    


/*******************************************************************************
* Function Name  : AccelerometerMovement
* Description    : Sets the create drive motors based on the orientation of the
*                  STM32 Primer 2.
*
* Input          : None
* Return         : None
*******************************************************************************/
/*
void AccelerometerMovement( void )
{
    tMEMS_Info* memsInfo;
    s16 leftVel, rightVel;
    
    memsInfo = MEMS_GetInfo();
    
    leftVel = 0;
    rightVel = 0;
    
    if( memsInfo->OutY <= -100 || memsInfo->OutY >= 100 && globals.frontPing > EMERGENCY_STOP_VALUE)
    {
        leftVel = memsInfo->OutY;
        rightVel = memsInfo->OutY;
    }
    
    if( memsInfo->OutX <= -100 )
    {
        if( memsInfo->OutY > -100 && memsInfo->OutY < 100 )
        {
            leftVel = -memsInfo->OutX;
            rightVel = memsInfo->OutX;
        }
        else if( rightVel > 0 )
        {
            leftVel -= memsInfo->OutX;
        }
        else if( rightVel < 0 )
        {
            leftVel += memsInfo->OutX;
        }
    }
    else if( memsInfo->OutX >= 100 )
    {
        if( memsInfo->OutY > -100 && memsInfo->OutY < 100 )
        {
            leftVel = -memsInfo->OutX;
            rightVel = memsInfo->OutX;
        }
        else if( leftVel > 0 )
        {
            rightVel += memsInfo->OutX;
        }
        else if( leftVel < 0 )
        {
            rightVel -= memsInfo->OutX;
        }
    }
    
    globals.leftVel = leftVel;
    globals.rightVel = rightVel;
    Create_DriveDirect(USART1, rightVel, leftVel);
}
*/


/*******************************************************************************
* Function Name  : updateStatus
* Description    : Displays useful information onto the LCD of the Primer 2
*               
*
* Input          : None
* Return         : None
*******************************************************************************/
void updateStatus( void )
{
    char buffer[6];
    u32 lastReading = 0;

    DRAW_SetTextColor(RGB_RED);
    
    UTIL_uint2str(buffer, globals.frontPing, 3, 1);
    DRAW_DisplayString(20, 100, buffer, 3);

    UTIL_uint2str(buffer, globals.rfPing, 3, 1);
    DRAW_DisplayString(20, 85, buffer, 3);

    UTIL_uint2str(buffer, globals.rrPing, 3, 1);
    DRAW_DisplayString(20, 70, buffer, 3);
    
    UTIL_int2str(buffer, globals.distance, 5, 1);
    DRAW_DisplayString(20, 55, buffer, 6);

    UTIL_int2str(buffer, globals.angle, 5, 1);
    DRAW_DisplayString(20, 40, buffer, 6);
}


/*******************************************************************************
* Function Name  : TIM1_Irq
* Description    : TIM1 Update interrupt, called each time the timer overflows.
*                  microSeconds is incremented each time this happens, keeping
*                  track of usecs as a 32-bit value.
*
* Input          : None
* Return         : None
*******************************************************************************/
tHandler TIM1_Irq(void)
{
    TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
    microSeconds += 0x00010000;
}


/*******************************************************************************
* Function Name  : getTimeMicro
* Description    : Returns the # of microseconds that have elapsed.
*               
*
* Input          : None
* Return         : None
*******************************************************************************/
u32 getTimeMicro( void )
{
    return microSeconds + TIM1->CNT;
}


void updateSensors( void )
{
    static u32 lastReading = 0;
    s16 dist;
    s16 angle;
    
    if( getTimeMicro() - lastReading < 15000 )
        return;
    
    u16 ping = PingPin(GPIOPing, FRONT_PING);
    globals.frontPingRaw = ping;
    globals.frontPing = (u16)convertTimeToCM(ping);
    
    ping = PingPin(GPIOPing, RF_PING);
    globals.rfPingRaw = ping;
    globals.rfPing = (u16)convertTimeToCM(ping);
    
    ping = PingPin(GPIOPing, RR_PING);
    globals.rrPingRaw = ping;
    globals.rrPing = (u16)convertTimeToCM(ping);
    
    Create_GetDistAngle(USART1, &dist, &angle);
    globals.distance += dist;
    globals.angle += angle;
    
    globals.bumpers = Create_GetBumpers(USART1);
    
    lastReading = getTimeMicro();
}


void alignWithWall( void )
{
    updateSensors();
    
    if( globals.rfPingRaw > globals.rrPingRaw )
    {
        Create_DriveDirect(USART1, -100, 100);
        while( globals.rfPingRaw > globals.rrPingRaw )
            updateSensors();
        updateStatus();
        Create_Drive(USART1, 0, 0);
    }
    else if( globals.rrPingRaw > globals.rfPingRaw )
    {
        Create_DriveDirect(USART1, 100, -100);
        while( globals.rrPingRaw > globals.rfPingRaw )
            updateSensors();
        updateStatus();
        Create_Drive(USART1, 0, 0);
    }
}

void followTheWall( void )
{
    DRAW_DisplayString(0, 15, "               ", 15);
    updateSensors();
    if((globals.rfPing > 100 && globals.rrPing > 100) || globals.frontPing <= 25)
        return;
    else
        Create_Drive(USART1, 500, 0x7FFF);
    while( (globals.rfPing < 100 || globals.rrPing < 100) )
    {
        updateSensors();
        updateStatus();
        if( globals.frontPing <= 20 )
        {
            DRAW_DisplayString(0, 15, "STOPPED-WALL", 12);
            Create_Drive(USART1, 0, 0);
            return;
        }
        s16 diff = globals.rfPing - globals.rrPing;
        diff = abs(diff);
        if( (diff > 2) && (diff < 5) )
        {
            DRAW_DisplayString(0, 15, "ALIGNING    ", 12);
            alignWithWall();
        }
        Create_Drive(USART1, 500, 0x7FFF);
    }
    DRAW_DisplayString(0, 15, "OPENING SEEN", 12);
    Create_Drive(USART1, 0, 0);
}


void followTheWall_grid( void )
{
    u8 done = 0;
    u8 state = 0;
    u8 speed = 250;
    
    while( !done )
    {
        updateSensors();
        updateStatus();
        
        if( globals.frontPing > 35 && globals.rfPing < 50 && globals.rrPing < 50 )
        {
            // align and go straight 2 feet if front is reading over 80cm
            alignWithWall();
            u16 dist = 610;
            u16 startDist = globals.distance;
            if( globals.frontPing < 81 )
                dist = (globals.frontPing - 30) * 10;
            Create_DriveDirect(USART1, speed, speed);
            while( globals.distance-startDist < dist )
            {
                updateSensors();
                updateStatus();
            }
        }
        else if(globals.frontPing > 35 && globals.rfPing > 80 && globals.rrPing < 50)
        {
            s16 angle = globals.angle;
            Create_DriveDirect(USART1, 0, speed);
            while( angle - globals.angle < 90 )
            {
                updateSensors();
                updateStatus();
            }
        }
        else if(globals.frontPing > 35 && globals.rfPing < 50 && globals.rrPing > 80)
        {
            Create_DriveDirect(USART1, speed, speed);
            while( globals.rrPing >= 50 )
            {
                updateSensors();
                updateStatus();
            }
        }
        else if(globals.frontPing <= 35 && globals.frontPing < 50 && globals.rrPing < 50)
        {
            s16 angle = globals.angle;
            Create_DriveDirect(USART1, speed, -speed);
            while ( globals.angle - angle < 90 )
            {
                updateSensors();
                updateStatus();
            }
        }
        else if(globals.rfPing > 80 && globals.rrPing > 80)
        {
            s16 angle = globals.angle;
            Create_DriveDirect(USART1, -speed, speed);
            while ( angle - globals.angle < 90 )
            {
                updateSensors();
                updateStatus();
            }

            u16 startDist = globals.distance;
            Create_DriveDirect(USART1, speed, speed);

            while( globals.distance-startDist < 305 )
            {
                updateSensors();
                updateStatus();
            }
        }
        else
        {
            Create_DriveDirect(USART1, 0, 0);
//            done = 1;
        }
    }
}
